/***************************************************************************
 创建者: 华磊
 开始时间: 2016.11.28
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
１）华磊，　2017.06.21把接口重新整理了一遍，以前在motorInterface的单位转换的函数去掉，集成到这里。
 ***************************************************************************
 *  @file jointdecouple.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef JOINTDECOUPLE_H
#define JOINTDECOUPLE_H

#include "GeneralDefine.h"
#include "dataanalysizerdouble.h"
#include "dataanalysizer.h"
#include "../AxisDefines.h"

class FilteringAlgorithm;

//using namespace KDL;

/**************************************
//单位统一说明：
//关节采用　s , mm ,　degree。
//电机采用    s,   puls　。
//力矩转换与单位无关,都可以兼容。
**************************************/

//QVector<double> tmpJointsPosition,tmpJointsVel,tmpJointAcc, tmpJointTorque;
//tmpJointsPosition.resize(totalAxisNumber);
//tmpJointsVel.resize(totalAxisNumber);
//tmpJointAcc.resize(totalAxisNumber);
//tmpJointTorque.resize(totalAxisNumber);
/**
 * @brief The JointDecouple class 关节数据解耦
 */
class  JointDecouple
{
public:
    JointDecouple(QVector<JointAttribute> *robotAttributeIn, int robotTypeIn,int robotIdIn,double deltaTimeIn);

    int setAxisRatedTorque(int axisIdIn,double rateTorqueIn);

    int getMotorEncoderPulsePerRound(QVector<double>& pulseOut);

    /**
     * @brief setDecoupleData　设置解偶数据
     * @param jointsPosition
     * @param jointsVel
     * @param jointAcc
     * @param jointTorque
     * @return
     */
    int setDecoupleData(const QVector<double>& jointsPosition, const QVector<double>& jointsVel
                        , const QVector<double>& jointAcc, const QVector<double>& jointTorque);
    int setDecoupleData_velCompensation(const QVector<double>& jointsVelCompensation);
//    int setDecoupleData_qqVel(const QVector<double>& qqVelIn);
    int setDecoupleData_torqueCompensation(const QVector<double>& jointsTorqueCompensation);
    /**
     * @brief executeDecoupleToPulse　执行解偶和单位转换
     * @return 　-1不支持的解耦类型　１成功
     */
    int executeDecoupleToPulse();
    /**
     * @brief getFeedbackJoint　根据电机脉冲经过解偶转换，获取反馈关节值，
     * @param jointsPosition
     * @param jointsVel
     * @param jointAcc
     * @param jointTorque
     * @return
     */
    int getFeedbackJoint(QVector<double>& jointsPosition,  QVector<double>& jointsVel, QVector<double> &jointAcc, QVector<double>& jointTorque);

    int getCommandJoint(QVector<double> &jointsPosition, QVector<double> &jointsVel, QVector<double> &jointAcc, QVector<double> &jointTorque);

     int executeReverseDecoupleToJoint();

    /**
     * @brief initialDataAnalysizer　重新初始化dataAnalysizer的数据
     * @return
     */
    int initialDataAnalysizer();

    QVector<MotorData>  getMotorDataCommandVector() ;

    QVector<MotorData>  getMotorDataFeedbackVector() ;

     int setMotorFeedbackDataVector(QVector<MotorData> motorFeedbackIn);

     int recordCurrentPosition2AbsoluteZeroMotorValue(QVector<int>& returnZeroMotorData);
     int recordCurrentPosition2AbsoluteZeroMotorValue(int &returnZeroMotorData, int axisId);
     int setAbsoluteZeroMotorValue(QVector<int> zeroMotorData);
     int setJointMechanicalZeroValue(QVector<double> jointValue);
     int translateJointToPulse(int axisId,double jointValueIn);


//    DataAnalysizerDouble* dataAnalysizerForJointCommand;
//    DataAnalysizerDouble* dataAnalysizerForJointFeedback;
//    DataAnalysizer* dataAnalysizerForMotorCommand;
//    DataAnalysizer* dataAnalysizerForMotorFeedback;

private:
    int decouple();//解耦关节到关节。减速比，耦合减速比都是>1的数。
    int reverseDecouple();//反解耦关节到关节。
    int translateJointToPulse();//转换关节到脉冲
    int translatePulseToJoint();//转换脉冲到关节
    int decoupleByOrder(QVector<int> deOrder);//根据参数顺序解耦
    int reverseDecoupleByOrder(QVector<int> deOrder);//根据参数顺序逆解耦

    /**
     * @brief addMsg  , 添加消息反馈
     * @param messageLevel
     * @param componentName
     * @param messageType
     * @param messageCode
     * @param robotId
     * @param parameter1
     * @param parameter2
     * @param parameter3
     * @param parameter4
     */
    void addMsg( int messageLevel,
                 std::string componentName,
                 std::string messageType,
                 int messageCode,
                 int robotId,
                 int parameter1 = 0,
                 int parameter2 = 0,
                 int parameter3 = 0,
                 int parameter4 = 0,
                 QString message= "");


public:
    int robotId;

    //未解偶的命令值。
    QVector<double> positionCommand;//位置命令　mm degree
    QVector<double> velCommand;//速度命令　mm/s degree/s
    QVector<double> velCompensation;//速度补偿　mm/s degree/s
//    QVector<double> qqvelCompensation;//根据位置变化计算的前馈速度补偿　mm/s degree/s
    QVector<double> accCommand;//加速度命令 mm/s^2 degree/s^2
    QVector<double> torqueCommand;//力矩命令　 N　Ｎ.mm
    QVector<double> torqueCompensation;//力矩补偿　 N　Ｎ.mm

    // 解偶后的命令值。
    QVector<double> positionCommandDecoupled;//位置命令　mm degree
    QVector<double> velCommandDecoupled;//速度命令　mm/s degree/s
    QVector<double> velCompensationDecoupled;//速度补偿　mm/s degree/s
    QVector<double> accCommandDecoupled;//加速度命令 mm/s^2 degree/s^2
    QVector<double> torqueCommandDecoupled;//力矩命令　N　Ｎ.mm
    QVector<double> torqueCompensationDecoupled;//力矩命令　N　Ｎ.mm

    //未解偶,反馈
    QVector<double> positionFeedbackOriginal;//反馈位置　mm degree
    QVector<double> velFeedbackOriginal;//反馈速度　mm/s degree/s
    QVector<double> accFeedbackOriginal;//没有使用
    QVector<double> torqueFeedbackOriginal;//反馈力矩　N　Ｎ.mm

    //已经解偶,反馈
    QVector<double> positionFeedbackDecoupled;//反馈位置　mm degree
    QVector<double> velFeedbackDecoupled;//反馈速度　mm/s degree/s
    QVector<double> accFeedbackDecoupled;//没有使用
    QVector<double> torqueFeedbackDecoupled;//反馈力矩　N　Ｎ.mm


    QVector<int> absoluteZeroMotorValue;//机器人ＤＨ算法零位电机值　pulse
    QVector<double> jointMechanicalZeroValue;//机器人机械零点关节值，相对于ＤＨ算法有偏移　mm degree,用于机器人校准零位（机械零位）与ＤＨ零位不重合的场景。

    volatile double deltaTime;//s

    QVector<FilteringAlgorithm*> filtering_feedback;

public:
    QVector<JointAttribute> *robotAttribute;//机器人关节属性指针
    QVector<MotorData>  *motorDataCommandVector;//电机命令值, pulse
    QVector<MotorData> *motorDataFeedbackVector;//电机反馈值指针，pulse
    int jointNum;//轴的个数
    int robotType;//机器人类型

//    struct MotorData
//    {
//        int position;//位置 　　（  puls编码器单位）
//        int speed;//速度　　     （puls/s)
//        int acceleration;//加速度　　     （puls/s^2)
//        int torque;//力矩             (0.1%　电机额定力矩千分比)
//        short torqueOffset;//   - 力距漂移　(0.1%　电机额定力矩千分比)
//        int velocityOffset;//   - 速度漂移 （puls/s)
//    };


};

#endif // JOINTDECOUPLE_H


